﻿using System;

using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;

class Matrix3
{
    public float M11, M12, M13;
    public float M21, M22, M23;
    public float M31, M32, M33;

    //empty constructor creates zero matrix
    public Matrix3()
    {
        setValues(0.0f, 0.0f, 0.0f,
                  0.0f, 0.0f, 0.0f,
                  0.0f, 0.0f, 0.0f);
    }

    //constructor, insert each matrix value
    public Matrix3(float m_11, float m_12, float m_13, float m_21, float m_22, float m_23, float m_31, float m_32, float m_33)
    {
        setValues(m_11, m_12, m_13,
                  m_21, m_22, m_23,
                  m_31, m_32, m_33);
    }

    //constructor , each vector 3 becomes column
    public Matrix3(Vector3 v1, Vector3 v2, Vector3 v3)
    {
        setValues(v1.X, v2.X, v3.X,
                  v1.Y, v2.Y, v3.Y,
                  v1.Z, v2.Z, v3.Z);
    }

    //set values
    public void setValues(float m_11, float m_12, float m_13, float m_21, float m_22, float m_23, float m_31, float m_32, float m_33)
    {
        M11 = m_11; M12 = m_12; M13 = m_13;
        M21 = m_21; M22 = m_22; M23 = m_23;
        M31 = m_31; M32 = m_32; M33 = m_33;
    }

    //matrix3 * vector3 operator for multiplication
    public static Vector3 operator *(Matrix3 m, Vector3 v)
    {
        Vector3 temp = new Vector3(
                           m.M11 * v.X + m.M12 * v.Y + m.M13 * v.Z,
                           m.M21 * v.X + m.M22 * v.Y + m.M23 * v.Z,
                           m.M31 * v.X + m.M32 * v.Y + m.M33 * v.Z

                           );

        return temp;

    }

    public static Matrix3 operator *(float h, Matrix3 m)
    {
        return new Matrix3(
                           m.M11 * h, m.M12 * h, m.M13 * h,
                           m.M21 * h, m.M22 * h, m.M23 * h,
                           m.M31 * h, m.M32 * h, m.M33 * h
                   );
    }

    public static Matrix3 operator +(Matrix3 mt_1, Matrix3 mt_2)
    {
        return new Matrix3(
                           mt_1.M11 + mt_2.M11, mt_1.M12 + mt_2.M12, mt_1.M13 + mt_2.M13,
                           mt_1.M21 + mt_2.M21, mt_1.M22 + mt_2.M22, mt_1.M23 + mt_2.M23,
                           mt_1.M31 + mt_2.M31, mt_1.M32 + mt_2.M32, mt_1.M33 + mt_2.M33
                   );
    }

    //matrix3 * operator for multiplication
    public static Matrix3 operator *(Matrix3 mt_1, Matrix3 mt_2)
    {

        return new Matrix3(
                           mt_1.M11 * mt_2.M11 + mt_1.M12 * mt_2.M21 + mt_1.M13 * mt_2.M31,
                           mt_1.M11 * mt_2.M12 + mt_1.M12 * mt_2.M22 + mt_1.M13 * mt_2.M32,
                           mt_1.M11 * mt_2.M13 + mt_1.M12 * mt_2.M23 + mt_1.M13 * mt_2.M33,

                           mt_1.M21 * mt_2.M11 + mt_1.M22 * mt_2.M21 + mt_1.M23 * mt_2.M31,
                           mt_1.M21 * mt_2.M12 + mt_1.M22 * mt_2.M22 + mt_1.M23 * mt_2.M32,
                           mt_1.M21 * mt_2.M13 + mt_1.M22 * mt_2.M23 + mt_1.M23 * mt_2.M33,

                           mt_1.M31 * mt_2.M11 + mt_1.M32 * mt_2.M21 + mt_1.M33 * mt_2.M31,
                           mt_1.M31 * mt_2.M12 + mt_1.M32 * mt_2.M22 + mt_1.M33 * mt_2.M32,
                           mt_1.M31 * mt_2.M13 + mt_1.M32 * mt_2.M23 + mt_1.M33 * mt_2.M33

                           );

    }

    public Matrix3 GramSchmidt()
    {
        Vector3 f1 = new Vector3(M11, M21, M31);
        Vector3 v2 = new Vector3(M12, M22, M32);
        Vector3 v3 = new Vector3(M13, M23, M33);

        f1.Normalize();

        Vector3 f2 = v2 - Vector3.Dot(v2, f1) * f1;
        f2.Normalize();

        Vector3 f3 = v3 - Vector3.Dot(v3, f1) * f1 - Vector3.Dot(v3, f2) * f2;

        f3.Normalize();

        return new Matrix3(f1.X, f2.X, f3.X,
                     f1.Y, f2.Y, f3.Y,
                     f1.Z, f2.Z, f3.Z);

    }

    public Matrix3 normalize()
    {
        float length_MX1 = (float)Math.Sqrt(Math.Pow(M11, 2) + Math.Pow(M21, 2) + Math.Pow(M31, 2));
        float length_MX2 = (float)Math.Sqrt(Math.Pow(M12, 2) + Math.Pow(M22, 2) + Math.Pow(M32, 2));
        float length_MX3 = (float)Math.Sqrt(Math.Pow(M13, 2) + Math.Pow(M23, 2) + Math.Pow(M33, 2));

        return new Matrix3(M11 / length_MX1, M12 / length_MX2, M13 / length_MX3,
                  M21 / length_MX1, M22 / length_MX2, M23 / length_MX3,
                  M31 / length_MX1, M32 / length_MX2, M33 / length_MX3);
    }

    public void rotationX(double angle)
    {
        //Console.WriteLine(angle);
        Identity(1.0f);
        M22 = (float)Math.Cos(angle);
        M23 = (float)-Math.Sin(angle);
        M32 = (float)Math.Sin(angle);
        M33 = (float)Math.Cos(angle);
    }

    public void rotationY(double angle)
    {
        //Console.WriteLine(angle);
        Identity(1.0f);
        M11 = (float)Math.Cos(angle);
        M13 = (float)Math.Sin(angle);
        M31 = (float)-Math.Sin(angle);
        M33 = (float)Math.Cos(angle);
    }

    public void rotationZ(double angle)
    {
        //Console.WriteLine(angle);
        Identity(1.0f);
        M11 = (float)Math.Cos(angle);
        M12 = (float)-Math.Sin(angle);
        M21 = (float)Math.Sin(angle);
        M22 = (float)Math.Cos(angle);
    }

    //get determinant for matrix
    public float Determinant()
    {
        float f_det;

        f_det = M11 * M22 * M33 + M21 * M32 * M13 + M31 * M12 * M23 - M11 * M32 * M23 - M31 * M22 * M13 - M21 * M12 * M33;

        return f_det;
    }

    //create inverse matrix
    public Matrix3 Inverse()
    {
        Matrix3 inv = new Matrix3();

        float detInv = 1 / this.Determinant();
        inv.M11 = detInv * (M22 * M33 - M23 * M32);
        inv.M12 = detInv * (M13 * M32 - M12 * M33);
        inv.M13 = detInv * (M12 * M23 - M13 * M22);

        inv.M21 = detInv * (M23 * M31 - M21 * M33);
        inv.M22 = detInv * (M11 * M33 - M13 * M31);
        inv.M23 = detInv * (M13 * M21 - M11 * M23);

        inv.M31 = detInv * (M21 * M32 - M22 * M31);
        inv.M32 = detInv * (M12 * M31 - M11 * M32);
        inv.M33 = detInv * (M11 * M22 - M12 * M21);

        return inv;
    }

    //write your matrix3 to console
    public override String ToString()
    {
        String s;

        s = "\n" + M11 + ", " + M12 + ", " + M13 + "\n" + M21 + ", " + M22 + ", " + M23 + "\n" + M31 + ", " + M32 + ", " + M33;

        return s;
    }

    //create a star matrix for 3x3 multiplications
    public void Star(Vector3 v)
    {
        setValues(0.0f, -v.Z, v.Y,
                  v.Z, 0.0f, -v.X,
                  -v.Y, v.X, 0.0f);
    }

    //make your matrix3 an identity matrix with desired value
    public void Identity(float value)
    {
        setValues(value * 1.0f, 0.0f, 0.0f,
                  0.0f, value * 1.0f, 0.0f,
                  0.0f, 0.0f, value * 1.0f);
    }

    //Create transverse matrix
    public Matrix3 Transverse()
    {
        Matrix3 M = new Matrix3();

        M.M11 = M11; M.M12 = M21; M.M13 = M31;
        M.M21 = M12; M.M22 = M22; M.M23 = M32;
        M.M31 = M13; M.M32 = M23; M.M33 = M33;

        return M;

    }

    public Quaternion Matrix3ToQuaternion()
    {
        Quaternion Q = new Quaternion();
        double trace;
        float s;

        trace = M11 + M22 + M33 + 1;

        if (trace >= 0)
        {
            s = (float)Math.Sqrt(trace);
            Q.W = 0.5f / s;
            s = 0.5f / s;
            Q.X = (M32 - M23) * s;
            Q.Y = (M13 - M31) * s;
            Q.Z = (M21 - M12) * s;
        }
        else
        {
            int i = 0;
            float x = M11;

            if (M22 > M11)
            {
                i = 1;
                x = M22;
            }
            if (M33 > x)
            {
                i = 2;
            }

            switch (i)
            {
                case 0:
                    s = (float)Math.Sqrt((M11 - (M22 + M33)) + 1.0f);
                    Q.X = 0.5f * s;
                    s = 0.5f / s;
                    Q.Y = (M12 + M21) * s;
                    Q.Z = (M31 + M13) * s;
                    Q.W = (M32 - M23) * s;
                    break;
                case 1:
                    s = (float)Math.Sqrt((M22 - (M33 + M11)) + 1.0f);
                    Q.Y = 0.5f * s;
                    s = 0.5f / s;
                    Q.Z = (M23 + M32) * s;
                    Q.X = (M12 + M21) * s;
                    Q.W = (M13 - M31) * s;
                    break;
                case 2:
                    s = (float)Math.Sqrt((M33 - (M11 + M22)) + 1.0f);
                    Q.Z = 0.5f * s;
                    s = 0.5f / s;
                    Q.X = (M31 + M13) * s;
                    Q.Y = (M23 + M32) * s;
                    Q.W = (M21 - M12) * s;
                    break;

            }
        }
        return Q;
    }

    public void QuaternionToMatrix3(Quaternion Q)
    {
        M11 = (1.0f - 2.0f * (Q.Y * Q.Y) - 2.0f * (Q.Z * Q.Z));
        M12 = (2.0f * (Q.X * Q.Y) - 2.0f * (Q.W * Q.Z));
        M13 = (2.0f * (Q.X * Q.Z) + 2.0f * (Q.W * Q.Y));
        M21 = (2.0f * (Q.X * Q.Y) + 2.0f * (Q.W * Q.Z));
        M22 = (1.0f - 2.0f * (Q.X * Q.X) - 2.0f * (Q.Z * Q.Z));
        M23 = (2.0f * (Q.Y * Q.Z) - 2.0f * (Q.W * Q.X));
        M31 = (2.0f * (Q.X * Q.Z) - 2.0f * (Q.W * Q.Y));
        M32 = (2.0f * (Q.Y * Q.Z) + 2.0f * (Q.W * Q.X));
        M33 = (1.0f - 2.0f * (Q.X * Q.X) - 2.0f * (Q.Y * Q.Y));

    }

    public Matrix getMatrix4()
    {
        return new Matrix(M11, M12, M13, 0f, M21, M22, M23, 0f, M31, M32, M33, 0f, 0f, 0f, 0f, 1f);
    }
}